// CVisMatrix.hpp
// Developed by Yang Yu (yuy@janelia.hhmi.org), Sept 27, 2012
// 

#ifndef __CVISMATRIX_HPP
#define __CVISMATRIX_HPP

#include "CVisMatrix.h"

//------------------
/// Matrix class
//------------------

template<class Tdata, class Tidx>
Y_MAT<Tdata, Tidx> :: Y_MAT()
{
    p=NULL; v=NULL; row=0; column=0;
}

template<class Tdata, class Tidx>
Y_MAT<Tdata, Tidx> :: ~Y_MAT()
{
}

template<class Tdata, class Tidx>
Y_MAT<Tdata, Tidx> :: Y_MAT(Tdata *pInput, Tidx dimx, Tidx dimy)
{
    p = pInput;
    row = dimx;
    column = dimy;

    // construct 2d pointer proxy
    try
    {
        v=new Tdata *[column];
        for(Tidx j=0; j<column; j++)
        {
            v[j] = p + j*row;
        }
    }
    catch(...)
    {
        cout<<"Fail to allocate memory for new Matrix!"<<endl;
        return;
    }
}

template<class Tdata, class Tidx>
void Y_MAT<Tdata, Tidx> :: setVal(Tidx r, Tidx c, Tdata val)
{
    if(r<0 || r>=row || c<0 || c>=column)
    {
        cout<<"Invalid index"<<endl;
        return;
    }

    v[c][r] = val; // show example here

    return;
}

template<class Tdata, class Tidx>
void Y_MAT<Tdata, Tidx> :: zeros()
{
    if(!p || !v)
    {
        cout<<"Matrix is NULL"<<endl;
        return;
    }

    for(Tidx j=0; j<column; j++)
        for(Tidx i=0; i<row; i++)
            v[j][i] = 0;

    return;
}

template<class Tdata, class Tidx>
void Y_MAT<Tdata, Tidx> :: unit()
{
    if(!p || !v)
    {
        cout<<"Matrix is NULL"<<endl;
        return;
    }

    if(column<row)
    {
        for(Tidx j=0; j<column; j++)
        {
            for(Tidx i=0; i<row; i++)
            {
                v[j][i] = 0;
            }

            v[j][j] = 1;
        }
    }
    else
    {
        for(Tidx i=0; i<row; i++)
        {
            for(Tidx j=0; j<column; j++)
            {
                v[j][i] = 0;
            }

            v[i][i] = 1;
        }
    }

    return;
}

template<class Tdata, class Tidx>
void Y_MAT<Tdata, Tidx> :: init(Tidx dimx, Tidx dimy, Tdata value)
{
    // value == 0 is a zero matrix
    // value == 1 is a unitary matrix
    // otherwise create a diagonal matrix

    row = dimx;
    column = dimy;

    Tidx N = row*column;
    y_new<Tdata, Tidx>(p, N);

    // construct 2d pointer proxy
    try
    {
        v=new Tdata *[column];
        for(Tidx j=0; j<column; j++)
        {
            v[j] = p + j*row;
        }
    }
    catch(...)
    {
        cout<<"Fail to allocate memory!"<<endl;
        return;
    }

    if(column<row)
    {
        for(Tidx j=0; j<column; j++)
        {
            for(Tidx i=0; i<row; i++)
            {
                v[j][i] = 0;
            }

            v[j][j] = (Tdata)value;
        }
    }
    else
    {
        for(Tidx i=0; i<row; i++)
        {
            for(Tidx j=0; j<column; j++)
            {
                v[j][i] = 0;
            }

            v[i][i] = (Tdata)value;
        }
    }
}

template<class Tdata, class Tidx>
void Y_MAT<Tdata, Tidx> :: assign(const Y_MAT M) const
{
    if(row!=M.row || column!=M.column)
    {
        cout<<"Fail to assign matrix"<<endl;
        return;
    }

    for(Tidx j=0; j<column; j++)
    {
        for(Tidx i=0; i<row; i++)
        {
            v[j][i] = M.v[j][i];
        }
    }

    return;
}

template<class Tdata, class Tidx>
void Y_MAT<Tdata, Tidx> :: clone(Y_MAT pIn, bool t)
{
    if(t) // transpose
    {
        row = pIn.column;
        column = pIn.row;
    }
    else
    {
        row = pIn.row;
        column = pIn.column;
    }

    Tidx N = row*column;

    try
    {
        p=new Tdata [N];
        v=new Tdata *[column];

        for(Tidx j=0; j<column; j++)
        {
            v[j] = p + j*row;
        }

        if(t)
        {
            for(Tidx j=0; j<column; j++)
            {
                for(Tidx i=0; i<row; i++)
                {
                    v[j][i] = pIn.v[i][j];
                }
            }
        }
        else
        {
            for(Tidx i=0; i<N; i++)
            {
                p[i] = pIn.p[i];
            }
        }

    }
    catch(...)
    {
        cout<<"Fail to allocate memory for new Matrix!"<<endl;
        return;
    }

    return;
}

template<class Tdata, class Tidx>
void Y_MAT<Tdata, Tidx> :: copy(Y_MAT M)
{
    clean();
    clone(M, false);
}

template<class Tdata, class Tidx>
void Y_MAT<Tdata, Tidx> :: clean()
{
    y_del<Tdata>(p);
    if(v){ delete [] v; v=NULL;} // because v is a 2D proxy pointer of p and p has been deleted
}

template<class Tdata, class Tidx>
void Y_MAT<Tdata, Tidx> :: prod(Y_MAT M)
{
    // inputs: this, M
    // output: M*this

    //
    if(column!=M.row)
    {
        cout<<"Matrix cannot multiply because dimensions are not match!"<<endl;
        return;
    }

    Y_MAT T;
    T.init(row,M.column, 0);

    // multiply
    for(Tidx k=0; k<M.column; k++)
    {
        for(Tidx i=0; i<row; i++)
        {
            Tdata sum=0;
            for(Tidx j=0; j<column; j++)
            {
                sum += v[j][i]*M.v[k][j];
            }
            T.v[k][i] = sum;
        }
    }

    //
    copy(T);
    T.clean();
}

template<class Tdata, class Tidx>
void Y_MAT<Tdata, Tidx> :: transpose()
{
    Y_MAT T; // tmp Matrix

    T.clone(*this, true);

    copy(T);

    T.clean();
}

template<class Tdata, class Tidx>
void Y_MAT<Tdata, Tidx> :: pseudoinverse()
{
    // A = U*S*V'
    // inv(A) = V*inv(S)*U'

    if(row!=column)
    {
        cout<<"Need a square matrix to decomposition!"<<endl;
        return;
    }

    //
    Tidx N = row;

    // init
    Y_MAT V;
    V.init(N, N, 0);
    Y_MAT S;
    S.init(N, N, 0);
    Y_MAT U;
    U.copy(*this);

    svd(U, S, V);

    // U'
    U.transpose();

    // inv(S)
    for(Tidx i=0; i<N; i++)
    {
        if(S.v[i][i]>EPS)
            S.v[i][i] = 1.0/S.v[i][i];
        else
            S.v[i][i] = 0.0;
    }

    // V*inv(S)*U'
    V.prod(S);
    V.prod(U);

    copy(V);

    //
    V.clean();
    S.clean();
    U.clean();
}

// computes (sqrt(a^2 + b^2)) without destructive underflow or overflow
// "Numerical Recipes" 1992
template<class Tdata, class Tidx>
Tdata Y_MAT<Tdata, Tidx> :: pythagorean(Tdata a, Tdata b)
{
    Tdata at = fabs(a), bt = fabs(b), ct, result;

    if (at > bt)       { ct = bt / at; result = at * sqrt(1.0 + ct * ct); }
    else if (bt > 0.0) { ct = at / bt; result = bt * sqrt(1.0 + ct * ct); }
    else result = 0.0;

    return result;
}

// if A=U*S*V'
//  INPUT: A (U=A, S=0, V=0)
// OUTPUT: U, S, V
// row=column case of the routine SVDCMP from "Numerical Recipes" 1992
template<class Tdata, class Tidx>
void Y_MAT<Tdata, Tidx> :: svd(Y_MAT U, Y_MAT S, Y_MAT V)
{
    if(row!=column)
    {
        cout<<"Need a square matrix to decomposition!"<<endl;
        return;
    }

    Tidx N = row;

    Tidx flag, i, its, j, jj, k, l, nm;
    Tdata anorm = 0.0, g = 0.0, scale = 0.0;

    Tdata c, f, h, s, x, y, z;
    Tdata *rv1=NULL;

    try
    {
        rv1 = new Tdata [N];
    }
    catch(...)
    {
        cout<<"Fail to allocate memory for temporary variable in svd function!"<<endl;
        y_del<Tdata>(rv1);
        return;
    }

    // Householder reduction to bidiagonal form
    for (i = 0; i < N; i++)
    {
        l = i + 1;
        rv1[i] = scale * g;

        // left-hand reduction
        g = s = scale = 0.0;
        if (i < N)
        {
            for (k = i; k < N; k++)
                scale += fabs(U.v[i][k]);
            if (scale)
            {
                for (k = i; k < N; k++)
                {
                    U.v[i][k] /= scale;
                    s += (U.v[i][k] * U.v[i][k]);
                }
                f = U.v[i][i];
                g = -y_sign2v<Tdata>(sqrt(s), f);
                h = f * g - s;
                U.v[i][i] = f - g;
                if (i != N - 1)
                {
                    for (j = l; j < N; j++)
                    {
                        for (s = 0.0, k = i; k < N; k++)
                            s += (U.v[i][k] * U.v[j][k]);
                        f = s / h;
                        for (k = i; k < N; k++)
                            U.v[j][k] += f * U.v[i][k];
                    }
                }
                for (k = i; k < N; k++)
                    U.v[i][k] *= scale;
            }
        }
        S.v[i][i] = scale * g;

        // right-hand reduction
        g = s = scale = 0.0;
        if (i < N && i != N - 1)
        {
            for (k = l; k < N; k++)
                scale += fabs(U.v[k][i]);
            if (scale)
            {
                for (k = l; k < N; k++)
                {
                    U.v[k][i] /= scale;
                    s += U.v[k][i] * U.v[k][i];
                }
                f = U.v[l][i];
                g = -y_sign2v<Tdata>(sqrt(s), f);
                h = f * g - s;
                U.v[l][i] = f - g;
                for (k = l; k < N; k++)
                    rv1[k] = U.v[k][i] / h;
                if (i != N - 1)
                {
                    for (j = l; j < N; j++)
                    {
                        for (s = 0.0, k = l; k < N; k++)
                            s += U.v[k][j] * U.v[k][i];
                        for (k = l; k < N; k++)
                            U.v[k][j] += s * rv1[k];
                    }
                }
                for (k = l; k < N; k++)
                    U.v[k][i] *= scale;
            }
        }
        anorm = y_max(anorm, (fabs(S.v[i][i]) + fabs(rv1[i])));
    }

    // accumulate the right-hand transformation
    for (i = N - 1; i >= 0; i--)
    {
        if (i < N - 1)
        {
            if (g)
            {
                for (j = l; j < N; j++)
                    V.v[i][j] = (U.v[j][i] / U.v[l][i]) / g;
                // double division to avoid underflow
                for (j = l; j < N; j++)
                {
                    for (s = 0.0, k = l; k < N; k++)
                        s += (U.v[k][i] * V.v[j][k]);
                    for (k = l; k < N; k++)
                        V.v[j][k] += (s * V.v[i][k]);
                }
            }
            for (j = l; j < N; j++)
                V.v[i][j] = V.v[j][i] = 0.0;
        }
        V.v[i][i] = 1.0;
        g = rv1[i];
        l = i;
    }

    // accumulate the left-hand transformation
    for (i = N - 1; i >= 0; i--)
    {
        l = i + 1;
        g = S.v[i][i];
        if (i < N - 1)
            for (j = l; j < N; j++)
                U.v[j][i] = 0.0;
        if (g)
        {
            g = 1.0 / g;
            if (i != N - 1)
            {
                for (j = l; j < N; j++)
                {
                    for (s = 0.0, k = l; k < N; k++)
                        s += (U.v[i][k] * U.v[j][k]);
                    f = (s / U.v[i][i]) * g;
                    for (k = i; k < N; k++)
                        U.v[j][k] += f * U.v[i][k];
                }
            }
            for (j = i; j < N; j++)
                U.v[i][j] *= g;
        }
        else
        {
            for (j = i; j < N; j++)
                U.v[i][j] = 0.0;
        }
        ++U.v[i][i];
    }

    // diagonalize the bidiagonal form
    for (k = N - 1; k >= 0; k--)
    {   // loop over singular values
        for (its = 0; its < 30; its++)
        {   // loop over allowed iterations
            flag = 1;
            for (l = k; l >= 0; l--)
            {   // Test for splitting
                // Note that rv1[1] is always zero
                nm = l - 1;
                if (fabs(rv1[l]) + anorm == anorm)
                {
                    flag = 0;
                    break;
                }
                if (fabs(S.v[nm][nm]) + anorm == anorm)
                    break;
            }
            if (flag)
            {
                c = 0.0;
                s = 1.0;
                for (i = l; i <= k; i++)
                {
                    f = s * rv1[i];
                    if (fabs(f) + anorm != anorm)
                    {
                        g = S.v[i][i];
                        h = pythagorean(f, g);
                        S.v[i][i] = h;
                        h = 1.0 / h;
                        c = g * h;
                        s = (- f * h);
                        for (j = 0; j < N; j++)
                        {
                            y = U.v[nm][j];
                            z = U.v[i][j];
                            U.v[nm][j] = y*c + z*s;
                            U.v[i][j] = z*c - y*s;
                        }
                    }
                }
            }
            z = S.v[k][k];
            if (l == k)
            {   //convergence
                if (z < 0.0)
                {   // singular value is made nonnegative
                    S.v[k][k] = -z;
                    for (j = 0; j < N; j++)
                        V.v[k][j] = -V.v[k][j];
                }
                break;
            }
            if (its >= 30) {
                y_del<Tdata>(rv1);
                cout<<"No convergence in 30 svdcmp iterations!"<<endl;
                return;
            }

            // shift from bottom 2-by-2 minor
            x = S.v[l][l];
            nm = k - 1;
            y = S.v[nm][nm];
            g = rv1[nm];
            h = rv1[k];
            f = ((y - z) * (y + z) + (g - h) * (g + h)) / (2.0 * h * y);
            g = pythagorean(f, 1.0);
            f = ((x - z) * (x + z) + h * ((y / (f + y_sign2v<Tdata>(g, f))) - h)) / x;

            // next QR transformation
            c = s = 1.0;
            for (j = l; j <= nm; j++)
            {
                i = j + 1;
                g = rv1[i];
                y = S.v[i][i];
                h = s * g;
                g = c * g;
                z = pythagorean(f, h);
                rv1[j] = z;
                c = f / z;
                s = h / z;
                f = x * c + g * s;
                g = g * c - x * s;
                h = y * s;
                y = y * c;
                for (jj = 0; jj < N; jj++)
                {
                    x = V.v[j][jj];
                    z = V.v[i][jj];
                    V.v[j][jj] = x*c + z*s;
                    V.v[i][jj] = z*c - x*s;
                }
                z = pythagorean(f, h);
                S.v[j][j] = z;
                if (z)
                {
                    z = 1.0 / z;
                    c = f * z;
                    s = h * z;
                }
                f = (c * g) + (s * y);
                x = (c * y) - (s * g);
                for (jj = 0; jj < N; jj++)
                {
                    y = U.v[j][jj];
                    z = U.v[i][jj];
                    U.v[j][jj] = y*c + z*s;
                    U.v[i][jj] = z*c - y*s;
                }
            }
            rv1[l] = 0.0;
            rv1[k] = f;
            S.v[k][k] = x;
        }
    }
    y_del<Tdata>(rv1);

    return;
}

// if a*x=b
//  INPUT: a, b
// OUTPUT: x
// Solve a linear equation system a*x=b using LU decomposition.
// Crout's algorithm from "Numerical Recipes" 1992
template<class Tdata, class Tidx>
void Y_MAT<Tdata, Tidx> :: lu(Y_MAT a, Y_MAT b)
{
    //
    /// Solve linear system: Anxn * Xdxn = Bdxn
    //

    if(a.row!=a.column)
    {
        cout<<"Invalid matrix dimensions!"<<endl;
        return;
    }

    Tidx n = a.row, d = b.row;
    Tdata pivsign = 1; // no row interchanges

    Tidx * piv = NULL;
    y_new<Tidx, Tidx>(piv, n);

    REAL *scalefactor = NULL;
    y_new<REAL, Tidx>(scalefactor, n);

    //
    /// LU DECOMPOSITION
    //

    // init ordering
    for (Tidx i = 0; i < n; ++i) piv[i] = i;

    // scaling factor
    for (Tidx i=0;i<n;i++)
    {
        REAL big=0.0;
        for (Tidx j=0;j<=n;j++)
        {
            Tdata val = a.v[i][j];

            if(y_abs<Tdata>(val)>big) big = val;
        }
        if (big == 0.0) throw("Singular matrix in LU decomposition");
        scalefactor[i]=1.0/big;
    }

    // decomposition
    for (Tidx k=0;k<n;k++)
    {
        Tidx imax=k;
        REAL big=0.0;

        for (Tidx i=k;i<n;i++) //search for largest pivot element
        {
            REAL temp=scalefactor[i]*y_abs<Tdata>(a.v[k][i]);
            if (temp > big)
            {
                big=temp;
                imax=i;
            }
        }
        if (k != imax) // interchange rows?
        {
            for (Tidx j=0;j<n;j++)
            {
                Tdata temp=a.v[j][imax];
                a.v[j][imax]=a.v[j][k];
                a.v[j][k]=temp;
            }
            pivsign = -(pivsign);
            scalefactor[imax]=scalefactor[k];
        }
        piv[k]=imax;
        if (a.v[k][k] == 0.0) a.v[k][k]=EPS;
        for (Tidx i=k+1;i<n;i++)
        {
            REAL temp = a.v[k][i] /= a.v[k][k];
            for (Tidx j=k+1;j<n;j++)
                a.v[j][i] -= temp*a.v[j][k];
        }
    }

    //
    /// SOLVE
    //

    Tidx ii = 0; // When ii is set to a positive value, it will become the index of thefirst nonvanishing element of b.

    //
    for(Tidx k = 0; k<d; k++)
    {
        for(Tidx i=0;i<n;i++)
        {
            Tidx ip=piv[i];
            Tdata sum=b.v[ip][k];
            b.v[ip][k]=b.v[i][k];
            if (ii!=0)
                for (Tidx j=ii-1;j<i;j++) sum -= a.v[j][i]*b.v[j][k];
            else if (sum!=0)
                ii=i+1;
            b.v[i][k]=sum;
        }
        for (Tidx i=n-1;i>=0;i--)
        {
            Tdata sum=b.v[i][k];
            for (Tidx j=i+1;j<n;j++) sum -= a.v[j][i]*b.v[j][k];
            b.v[i][k]=sum/a.v[i][i];
        }
    }

    // de-alloc
    y_del<Tidx>(piv);
    y_del<REAL>(scalefactor);

    //
    return;
}

// vector product
template<class Tdata, class Tidx>
void Y_MAT<Tdata, Tidx> :: vectorMul(REAL *in, REAL *&out)
{
    // input:  1d vector "in"
    // output: 1d vector "out"

    if(row!=column)
    {
        cout<<"Error: fail to run function vectorMul"<<endl;
        return;
    }
    Tidx sz = row;

    for(Tidx j=0; j<sz; j++)
    {
        out[j] = 0;
        for(Tidx i=0; i<sz; i++)
        {
            out[j] += v[j][i]*in[i];
        }
    }

    return;
}

//
template<class Tdata, class Tidx>
void Y_MAT<Tdata, Tidx> :: normalize3DAffine(Y_MAT M)
{
    // input:  M 3xn matrix
    // output: 4x4 affine matrix and normalized M

    //
    Tidx n = M.column;
    Tdata cx=0, cy=0, cz=0; // centroid
    for(Tidx i=0; i<n; i++)
    {
        cx+=M.v[i][0];
        cy+=M.v[i][1];
        cz+=M.v[i][2];
    }
    cx/=n;
    cy/=n;
    cz/=n;
    for(Tidx i=0; i<n; i++)
    {
        M.v[i][0]-=cx;
        M.v[i][1]-=cy;
        M.v[i][2]-=cz;
    }

    Tdata scaleGrid= 0;
    for(Tidx i=0; i<n; i++)
    {
        scaleGrid += sqrt(M.v[i][0]*M.v[i][0] + M.v[i][1]*M.v[i][1] + M.v[i][2]*M.v[i][2]);
    }
    scaleGrid = n/scaleGrid;
    for(Tidx i=0; i<n; i++)
    {
        M.v[i][0] *= scaleGrid;
        M.v[i][1] *= scaleGrid;
        M.v[i][2] *= scaleGrid;
    }

    //
    v[0][0] = scaleGrid;
    v[0][1] = 0;
    v[0][2] = 0;
    v[0][3] = -scaleGrid*cx;
    v[1][0] = 0;
    v[1][1] = scaleGrid;
    v[1][2] = 0;
    v[1][3] = -scaleGrid*cy;
    v[2][0] = 0;
    v[2][1] = 0;
    v[2][2] = scaleGrid;
    v[2][3] = -scaleGrid*cz;
    v[3][0] = 0;
    v[3][1] = 0;
    v[3][2] = 0;
    v[3][3] = 1;

    return;
}

template<class Tdata, class Tidx>
void Y_MAT<Tdata, Tidx> :: getRigid(Y_MAT M1, Y_MAT M2)
{
    // inputs: fields M1 & M2
    // output: rigid matrix

    //
    unit();

    //
    Y_MAT aff1, aff2;
    aff1.init(4, 4, 1); // 3D
    aff2.init(4, 4, 1);

    //
    Y_MAT m1, m2;
    m1.copy(M1);
    m2.copy(M2);

    aff1.normalize3DAffine(m1);
    aff2.normalize3DAffine(m2);

    //
    m2.transpose();
    m1.prod(m2); // 3x3 matrix

    //
    Y_MAT<REAL, Tidx> V;
    V.init(3, 3, 0);
    Y_MAT<REAL, Tidx> S;
    S.init(3, 3, 0);

    //
    m1.svd(m1, S, V);
    m1.transpose();
    V.prod(m1); // R

    //
    for(Tidx i=0; i<3; i++)
        for(Tidx j=0; j<3; j++)
            v[i][j] = V.v[i][j];

    // aff2-1*v*aff1
    aff2.pseudoinverse();
    aff2.prod(*this);
    aff2.prod(aff1);

    //
    assign(aff2);

    // de-alloc
    aff1.clean();
    aff2.clean();
    m1.clean();
    m2.clean();
    V.clean();
    S.clean();

    //
    return;
}

template<class Tdata, class Tidx>
void Y_MAT<Tdata, Tidx> :: sortEigenVectors(Y_MAT E)
{
    // sort by descend order
    //
    //EXAMPLE:
    // E =
    //    100     0     0
    //      0    25     0
    //      0     0    50
    // this =
    //      1     2     3
    //      1     2     3
    //      1     2     3
    //
    //sortEigenVectors(E)
    // E =
    //    100     0     0
    //      0    50     0
    //      0     0    25
    // this =
    //      1     3     2
    //      1     3     2
    //      1     3     2

    //
    if(E.row!=row || E.column!=column)
    {
        cout<<"Invalid inputs"<<endl;
        return;
    }

    //
    Y_MAT Ssorted;
    Ssorted.init(row, column, 1);

    Tidx N = row;

    Tdata *data=NULL;
    y_new<Tdata, Tidx>(data, N);

    foreach(N, i) // row < column
    {
        data[i] = E.v[i][i];
    }

    //
    quickSort<Tdata, Tidx>(data, 0, N-1, N);

    foreach(N, k)
    {
        Tdata val = data[k];

        Tidx idx = 0;
        foreach(N, n)
        {
            if(val == E.v[n][n])
            {
                idx = n; break;
            }
        }

        foreach(N, i)
        {
            Ssorted.v[i][k] = v[i][idx];
        }
    }

    assign(Ssorted);

    //
    Ssorted.clean();
    y_del<Tdata>(data);

    //
    return;
}

template<class Tdata, class Tidx>
void Y_MAT<Tdata, Tidx> :: sortEigenValues()
{
    //
    /// sort by descend order
    //
    //EXAMPLE:
    //
    //    100     0     0
    //      0    25     0
    //      0     0    50
    //
    //sortEigenValues():
    //
    //    100     0     0
    //      0    50     0
    //      0     0    25
    //

    Tidx N = row; // assume row <= column

    Tdata *data=NULL;
    y_new<Tdata, Tidx>(data, N);

    foreach(N, i)
    {
        data[i] = v[i][i];
    }

    //
    quickSort<Tdata, Tidx>(data, 0, N-1, N);

    foreach(N, i)
    {
        v[i][i] = data[i];
    }

    //
    y_del<Tdata>(data);

    //
    return;
}

#endif //__CVISMATRIX_HPP
